
#include<stdio.h>
#include<Mesh/libcell_iterator.h>
#include <Mesh_IO/Mesh_IO.h>
#include <tool/libcell_tools_kd_node.h>
#include<tool/libcell_tools_algorithm.h>
#include<Algorithm/points_cloud_algorithm.h>
#include "yuandong.h"
#define quote lib_cell_quote
#define _ReadPly_ LibCell_ReadPly_
#define _ReadPcd_ LibCell_ReadPcd_


// static inline int my_cmp(void * a,void* b)
// {
//     Int_RB_Tree* tree1=(Int_RB_Tree*)a,*tree2=(Int_RB_Tree*)b;
//     return (tree1->size>tree2->size);
// }

// Node* get_cluster_pionts_from_mesh1(Mesh * m,double r)
// {
//     Int_RB_Tree* tree=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
//     int_rb_tree_init(tree); 
//     int size=m->num_v(m);
//     template_v**values=(template_v**)malloc(sizeof(template_v*)*size);
//     int i=0;
//     for(auto it=m->v_begin(m);it!=m->v_end(m);it++)
//     {
//         values[i]=quote(it); 
//         i++;
//     }
//     KD_Node* kdtree=create_kd_tree(values,size,0);
//     free(values);

//     Node* re=NULL;
//     for(auto it= m->v_begin(m);it!=m->v_end(m);it++)
//     {
//         if(tree->find(tree,quote(it)->id)==NULL)
//         {
//             Node* find_ts=NULL;
//             Node* n= kd_tree_find_nearest_sphere(quote(it),r,kdtree );
//             for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
//             {
//                 template_v* v=(template_v*)(nit->value);
//                 Int_RB_Tree* tree1=(Int_RB_Tree*)(tree->find(tree,v->id));
//                 if( tree1!=NULL && node_find(find_ts,tree1)==NULL)
//                 {
//                     find_ts=node_overlying(find_ts,tree1);
//                 }
//             }
//             if(find_ts==NULL)
//             {
//                 Int_RB_Tree* tree1=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
//                 int_rb_tree_init(tree1);
//                 for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
//                 {
//                     template_v* v=(template_v*)(nit->value);
//                     tree->insert(tree,v->id,tree1);
//                     tree1->insert(tree1,v->id,v); 
//                 } 
//                 re=node_overlying(re,tree1);
//             }
//             else
//             {
//                 Node* nit=find_ts;
//                 Int_RB_Tree* tree1=(Int_RB_Tree*)(nit->value);
//                 nit=(Node*)(nit->Next); 

//                 for( ;nit!=NULL ; nit=(Node*)(nit->Next))
//                 {
//                     Int_RB_Tree*tree2=(Int_RB_Tree*)(nit->value);
//                     for(auto it1=tree2->begin(tree2);it1.it!=NULL;it1++)
//                     {
//                         template_v* v=(template_v*)(it1.second) ;
//                         tree1->insert(tree1,v->id,v);
//                         tree->insert(tree,v->id,tree1); 
//                     }

//                     re=node_delete_value(re,tree2);
//                     int_rb_tree_free(tree2);
//                 }
                
//                 for(nit=n;nit!=NULL;nit=(Node*)(nit->Next))
//                 {
//                     template_v* v=(template_v*)(nit->value);
                    
//                     tree1->insert(tree1,v->id,v);
//                     tree->insert(tree,v->id,tree1);
//                 }
//             }
//             free_node(find_ts);
//             free_node(n);
//         } 
//     }
//     free_kdnode(kdtree);
//     int_rb_tree_free(tree);
//     re=node_bub_sortn(re,my_cmp);
//     return re;
// }


void my_test()
{
    Mesh mesh;
    Mesh_init(&mesh);
    _ReadPly_(&mesh,"184205.ply");

    printf("num v:%d\n",mesh.num_v(&mesh));

    double *box=mesh_compute_bounding_box(&mesh);
    double temps[3]={box[3]-box[0],box[4]-box[1],box[5]-box[2]};

    for(int i=0;i<2;i++)
    {
        for(int j=0;j<2;j++)
        {
            if(temps[j]<temps[j+1])
            {
                double temp=temps[j];
                temps[j]=temps[j+1];
                temps[j+1]=temp;
            }
        } 
    }


    printf("%lf %lf %lf\n",temps[0],temps[1],temps[2]);
    //double * box=mesh_compute_bounding_box_from_node();

    Node* clus=get_cluster_pionts_from_mesh1(&mesh, 0.03* temps[1]);
    for(Node* nit=clus;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Int_RB_Tree* tree=(Int_RB_Tree*)(nit->value);
        printf("%d\n",tree->size);
    }

    Mesh_free(&mesh);
}







int main()
{
    int n[2];
    float* v;unsigned int* f;
    yuandong_func("tee.ply",&v,&f, n);

    
    Mesh mesh, mesh1;
    Mesh_init(&mesh);Mesh_init(&mesh1);
    _ReadPcd_(&mesh,"152904.pcd");
    
    //_ReadPcd_(&mesh1,"185243Cloud.pcd");
 


    Mesh_free(&mesh);Mesh_free(&mesh1);

    //my_test(); 
    printf("end\n");
    return 0;
}